#!/usr/bin/python

"""
six-dof-demo.py
Six Degree of Freedom (6DOF) Stewart Platform simulation using ode3d.

Written By:
    James Thomas
    Email: jim@houseoftechnology.org
    Web: http://mission-cognition.houseoftechnology.org/

Copyright 2009-2012

This file is part of the PandaODElib distribution.

This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public License
as published by the Free Software Foundation; either version 3 of
the License, or (at your option) any later version. The text of the
GNU Lesser General Public License is included with this library in
the file named LICENSE.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
"""

from math import pi, sin

from pandac.PandaModules import Quat, Vec3, OdeBallJoint, OdePlaneGeom, Mat4

import odelib
import ode3d
from ode3d.lib import Piston
import pvis

# http://en.wikipedia.org/wiki/Stewart_platform

# Forces at the min and max pistion positions
MIN_SPRING_FORCE = 200
MAX_SPRING_FORCE = 50

PLATFORM_DENSITY = 0.25 * 500.0 # Assumes something like a plywood box truss (75% void)
PLATFORM_LX = 1.0 # Width
PLATFORM_LY = 0.1 # Thickness
PLATFORM_LZ = 1 # Length


# Arranged as follows:
# +2z  2     a      3
#
#
#   0
#
#  -z  c     1      b
#
#     -x     0     +x

# Mounting point locations  -- numbers are on the platform, letters on the base
# These coordinates are relative to the body origins during construction (0,0,0)
xOffset = PLATFORM_LX / 2.0
zOffset = PLATFORM_LX * sin(pi/3.0) / 2.0

scale = 2.0

pistonLength = scale * xOffset * 2.0

pBa = (0, 0, scale * zOffset) # Make the base bigger than the platform
pBb = (scale * xOffset, 0, scale * -zOffset)
pBc = (scale * -xOffset, 0, scale * -zOffset)
pP1 = (0, 0, -zOffset)
pP2 = (-xOffset, 0, zOffset)
pP3 = (xOffset, 0, zOffset)

# Piston list by mounting point
# The letter and digit can be reversed to get the other mounting point
pistonNames = ('a2', 'a3', 'b1', 'b3', 'c1', 'c2')

# Make a mounting point dictionaries
baseMountingPoints = {  'a2': pBa, 'a3': pBa, 'b1': pBb, 'b3': pBb, 'c1': pBc, 'c2': pBc }
platformMountingPoints = {'1b': pP1, '1c': pP1, '2a': pP2, '2c': pP2, '3a': pP3, '3b': pP3 }

class MyUserHandler(ode3d.lib.UserHandler):
    def __init__(self, App):
        ode3d.lib.UserHandler.__init__(self, App)
        self.simstate = odelib.SimState()
        
    def UpdateSettings(self):
        ode3d.lib.UserHandler.UpdateSettings(self)
        if self.simstate.simLoop == 50:
            print 'Release'
            self.app.GetObj('platform').ReleaseFixed()
            
class MyApp(ode3d.lib.OdeApp):
    def __init__(self):
        fps = 30 # 30 frames per second
        dt = 1/60.0 # the target rate for our sim time step
        gravity = (0,-9.81,0)
        
        # Now we can initialize the base class
        ode3d.lib.OdeApp.__init__(self, dt, fps, Gravity=gravity, UserHandlerClass=MyUserHandler)

        # Create a plane geom which prevent the objects from falling forever
        floor = OdePlaneGeom(self.world.space, 0, 1, 0, 0)
        
        # Create a display object for the floor
        floorDisp = pvis.box(size=(10,.0001, 10), color=(.2,.2,.2))
        
        # Set the pvis scene so we won't have auto-scaling goofing us up.
        #world.scene.autoscale = False
        #world.scene.center = (0, 1, 0)
        #world.scene.range = 3
        
        # Create an axis reference, x = red, y = green, z = blue
        pvis.arrow(pos=(0,0,1), q=pvis.xAxis, shaftwidth=0.1, color=(1,0,0))
        pvis.arrow(pos=(0,0,1), q=pvis.yAxis, shaftwidth=0.1, color=(0,1,0))
        pvis.arrow(pos=(0,0,1), q=pvis.zAxis, shaftwidth=0.1, color=(0,0,1))
        
        # Create the base
        baseBody = ode3d.GDMBody(self.world)
        baseElement = ode3d.GDMElement().DefineBox(3000, 2, 0.02, 2)
        baseBody.AddElement('Base', baseElement)
        
        # Create attachment points.
        for name, location in baseMountingPoints.iteritems():
            baseBody.DefineConnectionPoint(name, location)
            s = pvis.sphere(pos=location, radius=0.1, color=(.8, .25, .25))
            baseBody.AddElement(name, ode3d.DisplayElement(s))
        
        baseBody.setPosition((0, 0, 0))
        baseBody.SetFixed()
        self.AddObj('base', baseBody)
        
        # Create the platform
        platformBody = ode3d.GDMBody(self.world)
        platformElement = ode3d.GDMElement().DefineBox(PLATFORM_DENSITY, PLATFORM_LX, PLATFORM_LY, PLATFORM_LZ)
        platformElement.GetDisplayObject().color = (.5, .5, .8, 1)
        platformBody.AddElement('Platform', platformElement)
        
        # Add orientation arrows
        ax = pvis.arrow(pos=(0,0.5*PLATFORM_LY,0), q=pvis.xAxis, length=0.35, shaftwidth=0.05, color=(1,0.2,0.2))
        platformBody.AddElement('X', ode3d.DisplayElement(ax))
        ay = pvis.arrow(pos=(0,0.5*PLATFORM_LY,0), q=pvis.yAxis, length=0.35, shaftwidth=0.05, color=(0.2,1,0.2))
        platformBody.AddElement('Y', ode3d.DisplayElement(ay))
        az = pvis.arrow(pos=(0,0.5*PLATFORM_LY,0), q=pvis.zAxis, length=0.35, shaftwidth=0.05, color=(0.2,0.2,1))
        platformBody.AddElement('Z', ode3d.DisplayElement(az))
        
        # Create attachment points.
        for name, location in platformMountingPoints.iteritems():
            platformBody.DefineConnectionPoint(name, location)
            s = pvis.sphere(pos=location, radius=PLATFORM_LY, color=(.8, .25, .25))
            platformBody.AddElement(name, ode3d.DisplayElement(s))
        
        platformBody.setPosition((0, pistonLength, 0))
        self.AddObj('platform', platformBody)

        # Temporary until the joints get setup, we will release it in a bit
        platformBody.SetFixed()
        
        # Create the pistons
        pistonDict = {}
        for name in pistonNames:
            revname = name[1] + name[0]
            piston = Piston(self.world, pistonLength, 0.04, MIN_SPRING_FORCE, MAX_SPRING_FORCE)
            
            # Connect the inner piston tip using a ball joint
            platformBody.Connect(OdeBallJoint(self.world), revname, *piston.GetInnerConnectionInfo())
        
            # Connect the outer piston tip using a ball joint
            baseBody.Connect(OdeBallJoint(self.world), name, *piston.GetOuterConnectionInfo())
        
            # Store the piston instance in the dictionary for use later
            #pistonDict[name] = piston
            self.AddObj(name, piston)

        # Create an initial camera position
        pvis.base.disableMouse()
        pvis.base.camera.setPos(0, 4, 8)
        pvis.base.camera.lookAt(0, 1, 0) #@UndefinedVariable -- lookAt(), more Panda3d magic?
        pvis.base.camera.setR(180) #@UndefinedVariable -- setR(), more Panda3d magic?

        # Turn mouse control back on
        mat=Mat4(pvis.base.camera.getMat()) #@UndefinedVariable -- getMat(), more Panda3d magic?
        mat.invertInPlace()
        pvis.base.mouseInterfaceNode.setMat(mat)
        pvis.base.enableMouse()

app = MyApp()
#app.Run(Static=True)
app.Run(0)
